#include "LSM303.h"
#include "delay.h"

// register addresses
enum regAddr
{
      TEMP_OUT_L        = 0x05, // D
      TEMP_OUT_H        = 0x06, // D

      STATUS_M          = 0x07, // D

      INT_CTRL_M        = 0x12, // D
      INT_SRC_M         = 0x13, // D
      INT_THS_L_M       = 0x14, // D
      INT_THS_H_M       = 0x15, // D

      OFFSET_X_L_M      = 0x16, // D
      OFFSET_X_H_M      = 0x17, // D
      OFFSET_Y_L_M      = 0x18, // D
      OFFSET_Y_H_M      = 0x19, // D
      OFFSET_Z_L_M      = 0x1A, // D
      OFFSET_Z_H_M      = 0x1B, // D
      REFERENCE_X       = 0x1C, // D
      REFERENCE_Y       = 0x1D, // D
      REFERENCE_Z       = 0x1E, // D

      CTRL0             = 0x1F, // D
      CTRL1             = 0x20, // D
      CTRL_REG1_A       = 0x20, // DLH, DLM, DLHC
      CTRL2             = 0x21, // D
      CTRL_REG2_A       = 0x21, // DLH, DLM, DLHC
      CTRL3             = 0x22, // D
      CTRL_REG3_A       = 0x22, // DLH, DLM, DLHC
      CTRL4             = 0x23, // D
      CTRL_REG4_A       = 0x23, // DLH, DLM, DLHC
      CTRL5             = 0x24, // D
      CTRL_REG5_A       = 0x24, // DLH, DLM, DLHC
      CTRL6             = 0x25, // D
      CTRL_REG6_A       = 0x25, // DLHC
      HP_FILTER_RESET_A = 0x25, // DLH, DLM
      CTRL7             = 0x26, // D
      REFERENCE_A       = 0x26, // DLH, DLM, DLHC
      STATUS_A          = 0x27, // D
      STATUS_REG_A      = 0x27, // DLH, DLM, DLHC

      OUT_X_L_A         = 0x28,
      OUT_X_H_A         = 0x29,
      OUT_Y_L_A         = 0x2A,
      OUT_Y_H_A         = 0x2B,
      OUT_Z_L_A         = 0x2C,
      OUT_Z_H_A         = 0x2D,

      FIFO_CTRL         = 0x2E, // D
      FIFO_CTRL_REG_A   = 0x2E, // DLHC
      FIFO_SRC          = 0x2F, // D
      FIFO_SRC_REG_A    = 0x2F, // DLHC

      IG_CFG1           = 0x30, // D
      INT1_CFG_A        = 0x30, // DLH, DLM, DLHC
      IG_SRC1           = 0x31, // D
      INT1_SRC_A        = 0x31, // DLH, DLM, DLHC
      IG_THS1           = 0x32, // D
      INT1_THS_A        = 0x32, // DLH, DLM, DLHC
      IG_DUR1           = 0x33, // D
      INT1_DURATION_A   = 0x33, // DLH, DLM, DLHC
      IG_CFG2           = 0x34, // D
      INT2_CFG_A        = 0x34, // DLH, DLM, DLHC
      IG_SRC2           = 0x35, // D
      INT2_SRC_A        = 0x35, // DLH, DLM, DLHC
      IG_THS2           = 0x36, // D
      INT2_THS_A        = 0x36, // DLH, DLM, DLHC
      IG_DUR2           = 0x37, // D
      INT2_DURATION_A   = 0x37, // DLH, DLM, DLHC

      CLICK_CFG         = 0x38, // D
      CLICK_CFG_A       = 0x38, // DLHC
      CLICK_SRC         = 0x39, // D
      CLICK_SRC_A       = 0x39, // DLHC
      CLICK_THS         = 0x3A, // D
      CLICK_THS_A       = 0x3A, // DLHC
      TIME_LIMIT        = 0x3B, // D
      TIME_LIMIT_A      = 0x3B, // DLHC
      TIME_LATENCY      = 0x3C, // D
      TIME_LATENCY_A    = 0x3C, // DLHC
      TIME_WINDOW       = 0x3D, // D
      TIME_WINDOW_A     = 0x3D, // DLHC

      Act_THS           = 0x3E, // D
      Act_DUR           = 0x3F, // D

      CRA_REG_M         = 0x00, // DLH, DLM, DLHC
      CRB_REG_M         = 0x01, // DLH, DLM, DLHC
      MR_REG_M          = 0x02, // DLH, DLM, DLHC

      SR_REG_M          = 0x09, // DLH, DLM, DLHC
      IRA_REG_M         = 0x0A, // DLH, DLM, DLHC
      IRB_REG_M         = 0x0B, // DLH, DLM, DLHC
      IRC_REG_M         = 0x0C, // DLH, DLM, DLHC

      WHO_AM_I_M        = 0x0F, // DLM
      WHO_AM_I          = 0x0F, // D

      TEMP_OUT_H_M      = 0x31, // DLHC
      TEMP_OUT_L_M      = 0x32, // DLHC


      // dummy addresses for registers in different locations on different devices;
      // the library translates these based on device type
      // value with sign flipped is used as index into translated_regs array

      OUT_X_H_M         = -1,
      OUT_X_L_M         = -2,
      OUT_Y_H_M         = -3,
      OUT_Y_L_M         = -4,
      OUT_Z_H_M         = -5,
      OUT_Z_L_M         = -6,
      // update dummy_reg_count if registers are added here!

      // device-specific register addresses

      DLH_OUT_X_H_M     = 0x03,
      DLH_OUT_X_L_M     = 0x04,
      DLH_OUT_Y_H_M     = 0x05,
      DLH_OUT_Y_L_M     = 0x06,
      DLH_OUT_Z_H_M     = 0x07,
      DLH_OUT_Z_L_M     = 0x08,

      DLM_OUT_X_H_M     = 0x03,
      DLM_OUT_X_L_M     = 0x04,
      DLM_OUT_Z_H_M     = 0x05,
      DLM_OUT_Z_L_M     = 0x06,
      DLM_OUT_Y_H_M     = 0x07,
      DLM_OUT_Y_L_M     = 0x08,

      DLHC_OUT_X_H_M    = 0x03,
      DLHC_OUT_X_L_M    = 0x04,
      DLHC_OUT_Z_H_M    = 0x05,
      DLHC_OUT_Z_L_M    = 0x06,
      DLHC_OUT_Y_H_M    = 0x07,
      DLHC_OUT_Y_L_M    = 0x08,

      D_OUT_X_L_M       = 0x08,
      D_OUT_X_H_M       = 0x09,
      D_OUT_Y_L_M       = 0x0A,
      D_OUT_Y_H_M       = 0x0B,
      D_OUT_Z_L_M       = 0x0C,
      D_OUT_Z_H_M       = 0x0D
};
	

#define Buf_SIZE  20	  

static int16_t  LSM303_FIFO[3][Buf_SIZE];
static uint8_t	Wr_IndexMag = 0;

//磁力计标定值
static int16_t	Mag_Offset_X = 0,Mag_Offset_Y = 0,Mag_Offset_Z = 0;
static float  Mag_Scale_X = 1.0f,Mag_Scale_Y = 1.0f,Mag_Scale_Z = 1.0f;


//添加一个新的值到 温度队列 进行滤波
static void LSM303_NewValMag(int16_t* buf,int16_t val) {
  	buf[Wr_IndexMag] = val;
}


static int16_t LSM303_GetAvg(int16_t* buf){
  int i;
	int32_t	sum = 0;
	for(i=0;i<Buf_SIZE;i++)
		sum += buf[i];
	sum = sum / Buf_SIZE;
	return (int16_t)sum;
}


/**
 * @Description 写LSM303寄存器
 */
static void LSM303_WriteReg(u8 reg, u8 data){
	LSM303_CSL();
	LSM303_WriteReg(reg,data);
	LSM303_CSH();	
}


/**
* @Description LSM303 初始化 数据输出速度100hz 磁场量程0-4 gauss
 */
void LSM303_Init(void){
	int16_t temp[3];
	int i;
	LSM303_WriteReg(CTRL2, 0x00);	//复位 选择SPI 4线模式
	LSM303_WriteReg(CTRL1, 0x00);	//关闭加速度计
	delay_ms(10);
	   
	// Magnetometer 磁力计高精度、输出速率100hz
	// M_RES = 11 (); M_ODR = 101 (100 Hz ODR)
	LSM303_WriteReg(CTRL5, 0x74);
	
	// 磁力计量程 4 guss
	LSM303_WriteReg(CTRL6, 0x20);
	
	// 磁力计工作于正常模式、连续输出数据
	LSM303_WriteReg(CTRL7, 0x00);
	delay_ms(10);
	
	//读取一组磁场数据
	for(i=0;i<Buf_SIZE;i++)
	{
		
		LSM303_readMag(temp);
	}
}


/**
 * @Description 读LSM303设备id
 */
u8 LSM303_readID(void)
{
	u8 id;
	LSM303_CSL();
	id = SPI3_ReadReg(0x0F);
	LSM303_CSH();
	return id;
}


/**
 * @Description 读LSM303磁场数据
* @param Mag 长度为3的数组，存放x，y，z轴磁场数据
 */
void LSM303_readMag(int16_t *Mag)
{
	
	u8 buf[6];
	int16_t temp[3];
	LSM303_CSL();
	//读取磁场数据到buf
	SPI3_ReadRegs(D_OUT_X_L_M|0x40,6,buf);
	LSM303_CSH();
	//真实磁场数据是原始数据的二进制补码
	temp[0] = -(int16_t)(buf[1] << 8 | buf[0]); //Mx
	temp[1] = -(int16_t)(buf[3] << 8 | buf[2]); //My
	temp[2] = (int16_t)(buf[5] << 8 | buf[4]);  //Mz
	
	//对数据进行平均滤波
	LSM303_NewValMag(&LSM303_FIFO[0][0],temp[0]);
	LSM303_NewValMag(&LSM303_FIFO[1][0],temp[1]);
	LSM303_NewValMag(&LSM303_FIFO[2][0],temp[2]);
	Wr_IndexMag = (Wr_IndexMag + 1) % Buf_SIZE;
	//返回滤波后的数据
	Mag[0] = (LSM303_GetAvg(&LSM303_FIFO[0][0])-Mag_Offset_X)*Mag_Scale_X;  //Mx
	Mag[1] = (LSM303_GetAvg(&LSM303_FIFO[1][0])-Mag_Offset_Y)*Mag_Scale_Y;  //My
	Mag[2] = (LSM303_GetAvg(&LSM303_FIFO[2][0])-Mag_Offset_Z)*Mag_Scale_Z;  //Mz

}


